/***************************************************************************
 创建者: 华磊
 开始时间: 2020.3.10
 copyright: (C) 华友高科
 ***************************************************************************/

#include "vehiclevelocitysmoother.h"
#include <algorithm>
#include <QDebug>

VehicleVelocitySmoother::VehicleVelocitySmoother(double deltaTimeIn,double speed_lim_xIn, double accel_lim_xIn,
                                                 double speed_lim_yIn, double accel_lim_yIn,
                                                 double speed_lim_rotateIn, double accel_lim_rotateIn,
                                                 double decel_factorIn)
{
    period=deltaTimeIn;

    speed_lim_x=speed_lim_xIn;
    accel_lim_x=accel_lim_xIn;
    speed_lim_y=speed_lim_yIn;
    accel_lim_y=accel_lim_yIn;
    speed_lim_rotate=speed_lim_rotateIn;
    accel_lim_rotate=accel_lim_rotateIn;
    decel_factor=decel_factorIn;

    decel_lim_x = decel_factor*accel_lim_x;
    decel_lim_y = decel_factor*accel_lim_x;
    decel_lim_rotate = decel_factor*accel_lim_rotate;

    shutdown_req=false;
    input_active=false;
    pr_next=0;
}

double VehicleVelocitySmoother::sign(double x)
{
    return x < 0.0 ? -1.0 : +1.0;
}

double VehicleVelocitySmoother::median(std::vector<double> values)
{

      // Return the median element of an doubles vector
      nth_element(values.begin(), values.begin() + values.size()/2, values.end());
      return values[values.size()/2];

}

void VehicleVelocitySmoother::setRawVelocityCommand(VehicleVelocity msg)
{
  // Estimate commands frequency; we do continuously as it can be very different depending on the
  // publisher type, and we don't want to impose extra constraints to keep this package flexible
    struct timespec       tmpCurrentTime;
    clock_gettime(CLOCK_MONOTONIC, &tmpCurrentTime);
    double tmpPassTime=(tmpCurrentTime.tv_sec-last_cb_time.tv_sec)+(tmpCurrentTime.tv_nsec-last_cb_time.tv_nsec)/1000000000.0;
  if (period_record.size() < PERIOD_RECORD_SIZE)
  {
    period_record.push_back(tmpPassTime);
  }
  else
  {
    period_record[pr_next] = tmpPassTime;
  }

  pr_next++;
  pr_next %= period_record.size();
  last_cb_time = tmpCurrentTime;

  if (period_record.size() <= PERIOD_RECORD_SIZE/2)
  {
    // wait until we have some values; make a reasonable assumption (10 Hz) meanwhile
    cb_avg_time = 0.1;
  }
  else
  {
    // enough; recalculate with the latest input
    cb_avg_time = median(period_record);
  }

  input_active = true;

  // Bound speed with the maximum values
  target_vel.x_move  =
      msg.x_move  > 0.0 ? std::min(msg.x_move,  speed_lim_x) : std::max(msg.x_move,  -speed_lim_x);
  target_vel.y_move  =
      msg.y_move  > 0.0 ? std::min(msg.y_move,  speed_lim_x) : std::max(msg.y_move,  -speed_lim_x);
  target_vel.z_rotate =
      msg.z_rotate > 0.0 ? std::min(msg.z_rotate, speed_lim_rotate) : std::max(msg.z_rotate, -speed_lim_rotate);
}


void VehicleVelocitySmoother::setAgvVelFeedback(VehicleVelocity msg)
{
    current_vel_feedback = msg;

  // ignore otherwise
}

void VehicleVelocitySmoother::calculateSmootherVelocityCommand(VehicleVelocity &velCommandOut)
{
      struct timespec       tmpCurrentTime;
      clock_gettime(CLOCK_MONOTONIC, &tmpCurrentTime);
      double tmpPassTime=(tmpCurrentTime.tv_sec-last_cb_time.tv_sec)+(tmpCurrentTime.tv_nsec-last_cb_time.tv_nsec)/1000000000.0;

    if ((input_active == true) && (cb_avg_time > 0.0) &&
        (tmpPassTime > std::min(3.0*cb_avg_time, 0.5)))
    {
      // Velocity input no active anymore; normally last command is a zero-velocity one, but reassure
      // this, just in case something went wrong with our input, or he just forgot good manners...
      // Issue #2, extra check in case cb_avg_time is very big, for example with several atomic commands
      // The cb_avg_time > 0 check is required to deal with low-rate simulated time, that can make that
      // several messages arrive with the same time and so lead to a zero median
      input_active = false;

        qDebug()<<"VehicleVelocitySmoother : message lost too much ";
        target_vel.x_move = 0;
        target_vel.y_move = 0;
        target_vel.z_rotate = 0;

    }

    if ( (input_active == true) && (cb_avg_time > 0.0) &&
        ((tmpPassTime > 5.0*cb_avg_time)     || // 5 missing msgs
          (std::abs(current_vel_feedback.x_move  - last_cmd_vel.x_move)  > 0.2) ||
          (std::abs(current_vel_feedback.z_rotate - last_cmd_vel.z_rotate) > 2.0)))
    {
      // If the publisher has been inactive for a while, or if our current commanding differs a lot
      // from robot velocity feedback, we cannot trust the former; relay on robot's feedback instead
      // TODO: current command/feedback difference thresholds are 진짜 arbitrary; they should somehow
      // be proportional to max v and w...
      // The one for angular velocity is very big because is it's less necessary (for example the
      // reactive controller will never make the robot spin) and because the gyro has a 15 ms delay
//      if ( !quiet ) {
//        // this condition can be unavoidable due to preemption of current velocity control on
//        // velocity multiplexer so be quiet if we're instructed to do so
//        ROS_WARN_STREAM("Velocity Smoother : using robot velocity feedback " <<
//                        std::string(robot_feedback == ODOMETRY ? "odometry" : "end commands") <<
//                        " instead of last command: " <<
//                        (ros::Time::now() - last_cb_time).toSec() << ", " <<
//                        current_vel_feedback.linear.x  - last_cmd_vel.linear.x << ", " <<
//                        current_vel_feedback.angular.z - last_cmd_vel.angular.z << ", [" << name << "]"
//                        );
//      }
        qDebug()<<"VehicleVelocitySmoother warnning:: message lost more than 5";
      last_cmd_vel = current_vel_feedback;
    }

    VehicleVelocity cmd_vel;

    if ((target_vel.x_move  != last_cmd_vel.x_move) ||
        (target_vel.z_rotate != last_cmd_vel.z_rotate))
    {
      // Try to reach target velocity ensuring that we don't exceed the acceleration limits
      cmd_vel=target_vel;

      double v_inc, w_inc, max_v_inc, max_w_inc;

      v_inc = target_vel.x_move - last_cmd_vel.x_move;
      if ( (current_vel_feedback.x_move*target_vel.x_move < 0.0))
      {
        // countermarch (on robots with significant inertia; requires odometry feedback to be detected)
        max_v_inc = decel_lim_x*period;
      }
      else
      {
        max_v_inc = ((v_inc*target_vel.x_move > 0.0)?accel_lim_x:decel_lim_x)*period;
      }

      w_inc = target_vel.z_rotate - last_cmd_vel.z_rotate;
      if ( (current_vel_feedback.z_rotate*target_vel.z_rotate < 0.0))
      {
        // countermarch (on robots with significant inertia; requires odometry feedback to be detected)
        max_w_inc = decel_lim_rotate*period;
      }
      else
      {
        max_w_inc = ((w_inc*target_vel.z_rotate > 0.0)?accel_lim_rotate:decel_lim_rotate)*period;
      }

      // Calculate and normalise vectors A (desired velocity increment) and B (maximum velocity increment),
      // where v acts as coordinate x and w as coordinate y; the sign of the angle from A to B determines
      // which velocity (v or w) must be overconstrained to keep the direction provided as command
      double MA = sqrt(    v_inc *     v_inc +     w_inc *     w_inc);
      double MB = sqrt(max_v_inc * max_v_inc + max_w_inc * max_w_inc);

      double Av = std::abs(v_inc) / MA;
      double Aw = std::abs(w_inc) / MA;
      double Bv = max_v_inc / MB;
      double Bw = max_w_inc / MB;
      double theta = atan2(Bw, Bv) - atan2(Aw, Av);

      if (theta < 0)
      {
        // overconstrain linear velocity
        max_v_inc = (max_w_inc*std::abs(v_inc))/std::abs(w_inc);
      }
      else
      {
        // overconstrain angular velocity
        max_w_inc = (max_v_inc*std::abs(w_inc))/std::abs(v_inc);
      }

      if (std::abs(v_inc) > max_v_inc)
      {
        // we must limit linear velocity
        cmd_vel.x_move  = last_cmd_vel.x_move + sign(v_inc)*max_v_inc;
      }

      if (std::abs(w_inc) > max_w_inc)
      {
        // we must limit angular velocity
        cmd_vel.z_rotate = last_cmd_vel.z_rotate + sign(w_inc)*max_w_inc;
      }

      last_cmd_vel = cmd_vel;
    }
    else if (input_active == true)
    {

    }

    velCommandOut=last_cmd_vel;
    velCommandOut.y_move=target_vel.y_move;//need todo

}

